package it.unicam.cs.cosy.bioshape.core.collisiondetection.narrowphase;

import hermesV3.util.HermesLogger;
import it.unicam.cs.cosy.bioshape.core.behaviours.ActiveSite;
import it.unicam.cs.cosy.bioshape.core.components.Entity;
import it.unicam.cs.cosy.bioshape.core.components.EntityPair;
import it.unicam.cs.cosy.bioshape.core.components.ShapedEntity;

import java.util.ArrayList;
import java.util.HashMap;
import java.util.HashSet;
import java.util.Iterator;
import java.util.List;
import java.util.Map;
import java.util.Set;

import javax.vecmath.Matrix4d;
import javax.vecmath.Point3d;
//Unused due to V-Clip problems with hashcodes. Needs V-Clip revision
//import vclip.ClosestFeaturesHT; 
import vclip.DistanceReport;

/**
 * Implements the narrow phase using {@code V-clip} package. The algorithm is
 * based on the "voronoi marching" approach firstly proposed in 1991 by Lin and
 * Canny in the algorithm of the same name. It is applied with a bisection
 * approach such that the timestep is continuously split until the {@code ftc}
 * is found. During timestep division colliding couples which are no more in
 * collision due to the discovery of the timestep are discarded.<br>
 * This implementation of the {@code #getFTC(List, double)} method executes an
 * iterative binary search over the timestep {@code t} to find the FTC. The
 * timestep is cut in half and collisions are checked at that time. If at the
 * chosen time entities are interpenetrating (not interpenetrating resp.) the
 * left interval (the right interval resp.) is further subdivided. The halving
 * continues up until the FTC is found, i.e. when all entities are near enough
 * w.r.t. a given {@code EPS}.<br>
 * During the halving, errors can occur. The most common case is that the
 * halving can continue until the interval is zeroed (a = b = 0.0). A typical
 * case is when one or more couples are already interpenetrating at the
 * beginning of the interval. To avoid this specific case a routine checks the
 * passed pairs and blocks the halving, even before is started, if at least one
 * pair is interpenetrating. Then, if the interpenetration does not exceed a
 * certain threshold (defined by the user), the collision is resolved.<br>
 * NB: the more these cases occurs, the more inaccurate have been the choices of
 * simulation world space, velocities, entities cardinality and dimensions.
 * Please review your input and make sure to maintain a simulation space not
 * over-crowded.
 * 
 * @author Federico Buti, Matteo Micheletti
 */
public class VClipNarrowPhase implements NarrowPhase {

	private double a, b;
	private List<EntityPair> pairs;
	private Map<Long, Matrix4d> hash;
	private Map<Long, Boolean> movedHash;
	private double EPS = 0.001; // TODO sto epsilon?
	boolean cancelNarrow = false;
	private Set<Entity> entities;
	private Matrix4d I;

	private enum State {
		BACK, FORTH
	}

	/**
	 * Creates the support structures for the calculation of the exact distance
	 * between pairs of {@code Entity} objects.
	 * 
	 * @param cardinality
	 *            Number of entities currently available in the
	 *            {@code LocalCoordinator}.
	 */
	public VClipNarrowPhase(int cardinality) {
		entities = new HashSet<Entity>((int) (cardinality * 1.5));
		hash = new HashMap<Long, Matrix4d>((int) (cardinality * 1.5));
		movedHash = new HashMap<Long, Boolean>((int) (cardinality * 1.5));
		I = new Matrix4d();
		I.setIdentity();
	}

	private void debugPrint() {
		for (EntityPair pair : pairs) {
			DistanceReport drep = new DistanceReport();
			drep.setFeaturePromotion(true, Math.toRadians(10));
			drep.setMaxPairDistance(EPS); // maximal distance
			drep.setMaxClosePairs(1); // closest features
			System.out.println("NarrowPhase - Distance "
					+ pair.getE1().getName()
					+ "--"
					+ pair.getE2().getName()
					+ ": "
					+ +((ShapedEntity) pair.getE1()).vclip(drep,
							((ShapedEntity) pair.getE2()), I, -1, null));
			// System.out.println("Distance report " + drep);
			System.out.println(((ShapedEntity) pair.getE1()).getTransform());
			System.out.println("vel: " + pair.getE1().getInstantVelocity());
			System.out.println(((ShapedEntity) pair.getE2()).getTransform());
			System.out.println("vel: " + pair.getE2().getInstantVelocity());
		}
	}

	/**
	 * Checks if the iterative halving must continue:
	 * <ul>
	 * <li>if the two indexes match (both at the beginning, both at the end of
	 * the interval or simply they are equal) the iteration must stop;</li>
	 * <li>if the set of colliding couples is empty, the iteration must stop;
	 * <li>if at least one collision distance is over the {@code EPS}, the
	 * iterations must continue;</li>
	 * <li>in all the other cases the halving must stop.</li>
	 * 
	 * @return {@code true} if the halving must continue, {@code false}
	 *         otherwise.
	 */
	private boolean continueHalving() {
		if (a == b) // interval is zeroed
			return false;
		// if (pairs.size() == 0) // TODO useless check
		// return false;
		for (int i = 0; i < pairs.size(); i++) { //
			// if (Math.abs(pairs.get(i).getDistance()) > EPS)
			if (pairs.get(i).getDistance() > EPS
					|| pairs.get(i).getDistance() < 0)
				return true;
		}
		return false;
	}

	/**
	 * Calculates distances between {@code EntityPolytree}s of each collision
	 * pair and set them into two sets: {@code left} if the entities are still
	 * interpenetrating or {@code right} if their distance is positive (no
	 * collision).
	 * 
	 * @return a {@code State} value indicating the direction of collision
	 *         detection checking
	 */
	private State removeNotCollidingEntities() {
		ArrayList<EntityPair> back = new ArrayList<EntityPair>(pairs.size());
		ArrayList<EntityPair> forth = new ArrayList<EntityPair>(pairs.size());
		//
		for (EntityPair pair : pairs) {
			ShapedEntity first = (ShapedEntity) pair.getE1();
			ShapedEntity second = (ShapedEntity) pair.getE2();

			// TODO Initial features needs the hashing..dependant on hashcodes
			// which does not work properly in the current V-Clip release
			// FeaturePair startFeature = new FeaturePair();
			DistanceReport drep = new DistanceReport();
			drep.setFeaturePromotion(true, Math.toRadians(10));
			drep.setMaxPairDistance(EPS); // maximal distance
			drep.setMaxClosePairs(1); // closest features

			double distance = 0;
			// distlimit is negative (-1) to ensure that the exact distance and
			// the exact features of composing polytrees are returned in the
			// check
			distance = first.vclip(drep, second, I, -1, null);
			// System.out.println(distance);
			// TODO controllo sulla distanza per il bug
			if (checkDistance(pair, new Point3d[] { drep.getClosestPair().pnt1,
					drep.getClosestPair().pnt2 }, distance)) {
				pair.setDistance(distance);
				pair.setDistanceReport(drep);
			} else {
				HermesLogger.log(HermesLogger.ERROR, "Clipping error: the "
						+ "contact points have changed. ");
				this.cancelNarrow = true;
			}
			// TODO per il bug degli spigoli
			pair.saveClosestPointsDistances(
					new Point3d[] { drep.getClosestPair().pnt1,
							drep.getClosestPair().pnt2 }, distance);
			if (distance <= 0)
				back.add(pair);
			else
				forth.add(pair);
		}
		// moving to the right of the interval...
		if (back.size() == 0)
			return State.FORTH;
		// else moving to the left, eventually removing elements "forth" in time
		// but that are not near enough (distance > EPS) --> useful in small
		// steps
		for (Iterator<EntityPair> i = forth.iterator(); i.hasNext();) {
			EntityPair p = i.next();
			if (Math.abs(p.getDistance()) < EPS)
				i.remove();
		}
		pairs.removeAll(forth);
		return State.BACK;
	}

	/**
	 * Moves the entities of the passed delta time in the whole current
	 * timestep. Before that, entities are brought back to their positions at
	 * the beginning of the timestep, to limit error propagation in repeated
	 * matrix calculations.
	 * 
	 * @param t
	 *            the delta time of movement of the current timestep
	 */
	@SuppressWarnings("boxing")
	private void resetMoveEntities(double t) {
		// reset movement flags
		for (long id : movedHash.keySet())
			movedHash.put(id, new Boolean(false));
		// moves but before set the entity to the original position
		for (EntityPair pair : pairs) {
			if (movedHash.get(pair.getE1().getId()).booleanValue() == false) {
				ShapedEntity e1 = (ShapedEntity) pair.getE1();
				e1.setTransform(hash.get(e1.getId()));
				e1.move(t);
				movedHash.put(e1.getId(), new Boolean(true));
			}
			if (movedHash.get(pair.getE2().getId()).booleanValue() == false) {
				ShapedEntity e2 = (ShapedEntity) pair.getE2();
				e2.setTransform(hash.get(e2.getId()));
				e2.move(t);
				movedHash.put(e2.getId(), new Boolean(true));
			}
		}
	}

	/**
	 * Stores all the entities involved in the possible collisions and also
	 * creates an hashing based on the {@code id} of the entities to fast recall
	 * their original {@code center} and {@code transformation matrix}, before
	 * the narrow phase checking. Finally, it creates an hashing to keep track
	 * of moved entities during each iteration of the narrow phase.
	 */
	private void backUpAndHash() {
		boolean first, second;
		for (EntityPair ep : pairs) {
			first = entities.add(ep.getE1());
			second = entities.add(ep.getE2());
			if (first) {
				Long id = new Long(ep.getE1().getId());
				Matrix4d m = new Matrix4d(
						((ShapedEntity) ep.getE1()).getTransform());
				hash.put(id, m);
				movedHash.put(id, null); // set in movement method
			}
			if (second) {
				Long id = new Long(ep.getE2().getId());
				Matrix4d m = new Matrix4d(
						((ShapedEntity) ep.getE2()).getTransform());
				hash.put(id, m);
				movedHash.put(id, null); // set in movement method
			}
		}
	}

	/**
	 * Restores entity data (center and transformation matrix) on the basic of
	 * the initial hashing. This method <i>must</i> be called after all the non
	 * restorable entities have been removed.
	 */
	@SuppressWarnings("boxing")
	private void restore() {
		for (Entity pe : entities)
			((ShapedEntity) pe).setTransform(hash.get(pe.getId()));
	}

	/**
	 * Checks the previously calculated distance with the newly calculated one.
	 * This check is necessary because V-Clip returns different distances
	 * sometimes by exchanging the chosen closest features.
	 * 
	 * @param pair
	 *            The colliding entities.
	 * @param closestPoints
	 *            The two newly found closest points.
	 * @param distance
	 *            The newly calculated distance.
	 * @return {@code true} if the new distance is shorter than the older one,
	 *         {@code false} otherwise.
	 */
	// TODO forse lo tagliamo dalle palle eh! Me pare na minchiata.
	private boolean checkDistance(EntityPair pair, Point3d[] closestPoints,
			double distance) {
		if (distance > 0)
			return true; // Obviously!
		if ((closestPoints[0].getX() - closestPoints[1].getX()) == 0.0
				&& pair.getPreviousDistances()[0][1] != 0.0) {
			System.out.println("CAMBIATI PUNTI DI CONTATTO, FALSE su X");
			return false;
		}
		if ((closestPoints[0].getY() - closestPoints[1].getY()) == 0.0
				&& pair.getPreviousDistances()[1][1] != 0.0) {
			System.out.println("CAMBIATI PUNTI DI CONTATTO, FALSE su Y");
			return false;
		}
		if ((closestPoints[0].getZ() - closestPoints[1].getZ()) == 0.0
				&& pair.getPreviousDistances()[2][1] != 0.0) {
			System.out.println("CAMBIATI PUNTI DI CONTATTO, FALSE su Z");
			return false;
		}
		return true;
	}

	@Override
	public double getFTC(List<EntityPair> p, double t)
			throws SearchingErrorException {
		if (p.size() == 0)
			return Double.POSITIVE_INFINITY;
		// else
		pairs = p;
		debugPrint();
		entities.clear();
		hash.clear();
		movedHash.clear();
		// stores entities, backs up matrix4d
		backUpAndHash();
		double ftc, c;
		a = 0;
		// start iterations by checking collisions at the end of the timestep t
		ftc = b = c = t;
		resetMoveEntities(b); // moves to the end of the interval (b)
		State result = removeNotCollidingEntities();
		// iterative halving...
		boolean doContinue = continueHalving();
		//
		// continue + BACK ---> if at least one couple has distance < EPS
		if (doContinue && result.equals(State.BACK)) {
			// iteration over the time step interval
			do {
				ftc = c = (a + b) / 2;
				resetMoveEntities(c);
				result = removeNotCollidingEntities();
				if (cancelNarrow) // VCLIP bug
					break;
				if (result.equals(State.BACK))
					b = c; // checks first interval: A <--> C - b
				else
					a = c; // checks second interval: a - C <--> B
			} while (continueHalving());
			// check for errors ---> tunnelling or interval zeroing!
			if (cancelNarrow || b == 0) {
				restore();
				throw new SearchingErrorException(
						"Oops! That's embarrassing... " + a + " - " + b);
			}
			// continue + FORTH ---> all distances > 0, check which > EPS
		} else if (doContinue && result.equals(State.FORTH)) {
			for (Iterator<EntityPair> i = pairs.iterator(); i.hasNext();)
				if (i.next().getDistance() > EPS)
					i.remove();
			// if all > EPS (pairs empty) --> NO COLLISION in T
			ftc = pairs.isEmpty() ? Double.POSITIVE_INFINITY : t;
			//
			// !continue ---> for all distances d, 0 < d < EPS; d can be
			// negative but still abs(d) < EPS ---> all valid collisions.
		} else
			ftc = t;
		//
		// executes the restore on the remaining entities (we need to know from
		// the GC if a previously found FTC on another LC invalidate our FTC
		System.out.println("----");
		System.out.println("FTC = " + ftc);
		debugPrint();
		restore();
		return ftc;
	}

	@Override
	public List<EntityPair> getPairs() {
		return pairs;
	}

	@Override
	public ActiveSite[] getContactSurfaces() {
		// TODO Auto-generated method stub
		return null;
	}
}